using BepuPhysics.CollisionDetection;
using BepuUtilities;
using System.Diagnostics;
using System.Runtime.CompilerServices;

namespace BepuPhysics
{
    public partial class Solver
    {
        struct IncrementalContactDataUpdateFilter : ITypeBatchSolveFilter
        {
            public bool AllowFallback { get { return false; } }

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public bool AllowType(int typeId)
            {
                return NarrowPhase.IsContactConstraintType(typeId);
            }
        }

        struct IncrementalContactUpdateStageFunction : IStageFunction
        {
            public float Dt;
            public float InverseDt;

            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public void Execute(Solver solver, int blockIndex)
            {
                ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex];
                ref var typeBatch = ref solver.ActiveSet.Batches[block.BatchIndex].TypeBatches[block.TypeBatchIndex];
                solver.TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, solver.bodies, Dt, InverseDt, block.StartBundle, block.End);
            }
        }

        void IncrementalContactUpdateWorker(int workerIndex)
        {
            int start = GetUniformlyDistributedStart(workerIndex, context.ConstraintBlocks.Blocks.Count, context.WorkerCount, 0);

            int syncStage = 0;
            // 在每次使用两个乒乓球声明缓冲区之后,声明和未声明的状态交换。
            int claimedState = 1;
            int unclaimedState = 0;
            var bounds = context.WorkerBoundsA;
            var boundsBackBuffer = context.WorkerBoundsB;
            // 请注意,每个批次都有不同的开始位置。每个包含不同的约束子集,因此它们需要不同的起始位置。
            // 同样的概念也适用于前一步--前一步一次涵盖所有约束,而不是一批一批地覆盖。
            var incrementalContactUpdateStage = new IncrementalContactUpdateStageFunction { Dt = context.Dt, InverseDt = 1f / context.Dt };
            Debug.Assert(ActiveSet.Batches.Count > 0, "Don't dispatch if there are no constraints.");
            // 从技术上讲,这可能会改变前一步的开始,但目前我们重建的每一帧都开始了,所以无论以哪种方式都无关紧要。
            ExecuteStage(ref incrementalContactUpdateStage, ref context.ConstraintBlocks, ref bounds, ref boundsBackBuffer, workerIndex, 0, context.ConstraintBlocks.Blocks.Count,
                ref start, ref syncStage, claimedState, unclaimedState);
        }

        internal void IncrementallyUpdateContactConstraints(float dt, IThreadDispatcher threadDispatcher = null)
        {
            if (threadDispatcher == null)
            {
                var inverseDt = 1f / dt;
                ref var activeSet = ref ActiveSet;
                for (int i = 0; i < activeSet.Batches.Count; ++i)
                {
                    ref var batch = ref activeSet.Batches[i];
                    for (int j = 0; j < batch.TypeBatches.Count; ++j)
                    {
                        ref var typeBatch = ref batch.TypeBatches[j];
                        if (NarrowPhase.IsContactConstraintType(typeBatch.TypeId))
                        {
                            TypeProcessors[typeBatch.TypeId].IncrementallyUpdateContactData(ref typeBatch, bodies, dt, inverseDt, 0, typeBatch.BundleCount);
                        }
                    }
                }
            }
            else
            {
                ExecuteMultithreaded<IncrementalContactDataUpdateFilter>(dt, threadDispatcher, incrementalContactUpdateWorker);
            }
        }

    }
}
